package com.example.ar_10;

import android.app.Activity;
import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.ImageFormat;
import android.hardware.Camera;
import android.renderscript.Allocation;
import android.renderscript.Element;
import android.renderscript.RenderScript;
import android.renderscript.ScriptIntrinsicYuvToRGB;
import android.renderscript.Type;
import android.util.Log;
import android.view.Surface;
import android.view.SurfaceHolder;

import org.opencv.android.Utils;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;

import java.io.IOException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;

public class MyCamera {

    static final String TAG = "CameraTag";
    public Camera mCamera;
    public byte[] mData = null;
    public int w = 0;
    public int h = 0;
    int mCurrentCamIndex = 0;
    Context mContext;
    private RenderScript rs;
    private ScriptIntrinsicYuvToRGB yuvToRgbIntrinsic;
    private Type.Builder yuvType;
    private Allocation in, out;
    private boolean mPatternWasFound = false;
    private final org.opencv.core.Size mPatternSize = new org.opencv.core.Size(6, 9);
    private final MatOfPoint2f mCorners = new MatOfPoint2f();
    private static MainActivity activity;

    private Mat Rvec, Tvec;
    Mat rotMat = new Mat(3, 3, CvType.CV_32FC1);
    private final float[] model_view_matrix = new float[16];
    Camera.Parameters parameters;

    public MyCamera() {

    }

    public Context getContxt() {
        return mContext;
    }

    public void setContxt(Context contxt) {
        this.mContext = (MainActivity) contxt;
    }

    public Activity getActivity() {
        return (MainActivity) activity;
    }

    public void setActivity(Activity activity) {
        MyCamera.activity = (MainActivity) activity;
    }

    //将MainActivity传递到类中使用
    public MyCamera(Context context, MainActivity activity) {
        this.setContxt(context);
        this.setActivity(activity);
    }

    //初始化相机
    public Camera initCamera() {
        int cameraCount = 0;
        Camera cam = null;
        Camera.CameraInfo cameraInfo = new Camera.CameraInfo();
        cameraCount = Camera.getNumberOfCameras();

        for (int camIdx = 0; camIdx < cameraCount; camIdx++) {
            Camera.getCameraInfo(camIdx, cameraInfo);

            //在这里打开的是前置摄像头,可修改打开后置OR前置
            if (cameraInfo.facing == Camera.CameraInfo.CAMERA_FACING_BACK) {
                try {
                    Log.i(TAG, "初始化相机 CameraIndex " + camIdx);
                    cam = Camera.open(camIdx);
                    mCurrentCamIndex = camIdx;
                } catch (RuntimeException e) {
                    Log.e(TAG, "Camera failed to open: " + e.getLocalizedMessage());
                }
            }
        }
        return cam;
    }

    /**
     * 停止相机
     */
    public void stopCamera() {
        if (mCamera != null) {
            mCamera.setPreviewCallback(null);
            mCamera.stopPreview();
            mCamera.release();
            Log.i(TAG, "StopCamera: 停止预览并释放资源");
            mCamera = null;
        }
    }

    /**
     * 旋转屏幕后自动适配（若只用到竖的，也可不要）
     * 已经在manifests中让此Activity只能竖屏了
     *
     * @param activity 相机显示在的Activity
     * @param cameraId 相机的ID
     * @param camera   相机对象
     */
    public static void setCameraDisplayOrientation(Activity activity, int cameraId, Camera camera) {
        Camera.CameraInfo info = new Camera.CameraInfo();
        Camera.getCameraInfo(cameraId, info);
        int rotation = activity.getWindowManager().getDefaultDisplay().getRotation();
        int degrees = 0;
        switch (rotation) {
            case Surface.ROTATION_0:
                degrees = 0;
                break;
            case Surface.ROTATION_90:
                degrees = 90;
                break;
            case Surface.ROTATION_180:
                degrees = 180;
                break;
            case Surface.ROTATION_270:
                degrees = 270;
                break;
        }
        int result;
        if (info.facing == Camera.CameraInfo.CAMERA_FACING_BACK) {
            result = (info.orientation + degrees - 180) % 360;
            result = (360 - result) % 360;  // compensate the mirror
        } else {
            // back-facing
            result = (info.orientation - degrees + 360) % 360;
        }
        camera.setDisplayOrientation(result);
    }

    public void openCam(SurfaceHolder holder, Context mContext) {
        this.mContext = mContext;

        if (mCamera == null) {
            mCamera = initCamera();
            Log.i(TAG, "mCamera: " + mCamera.toString());
        }

        try {
            //适配竖排固定角度
            Log.i(TAG, "mCamera: " + mCamera.toString());
            setCameraDisplayOrientation((Activity) mContext, mCurrentCamIndex, mCamera);
            mCamera.setPreviewDisplay(holder);
            Log.i(TAG, "开始预览");


        } catch (IOException e) {
            mCamera.release();
            mCamera = null;
        }

        parameters = mCamera.getParameters();

    }

    public void everyTime() {
        mCamera.setPreviewCallback(new Camera.PreviewCallback() {
            @Override
            public void onPreviewFrame(byte[] data, Camera camera) {

                Camera.Size size = parameters.getPreviewSize();

                w = size.width;//2176
                h = size.height;//1080
                Log.d(TAG, "CAMERAINFO: " + w + "," + h);
                mData = data;


                /***********************************/

                //将预览数据从NV21转换到Bitmap
                //这个方式效率低，是android自带的函数
//                YuvImage image = new YuvImage(data, ImageFormat.NV21, w, h, null);
//                ByteArrayOutputStream stream = new ByteArrayOutputStream();
//                image.compressToJpeg(new Rect(0, 0, w, h), 80, stream);
//                Bitmap bmp = BitmapFactory.decodeByteArray(stream.toByteArray(), 0, stream.size());

                //此方法为高效NV21转换到Bitmap转换方法
                NV21ToBitmap(mContext);
                Bitmap bmp = nv21ToBitmap(data, w, h);

                Mat mat = new Mat();//2176,1080

                //opencv能够识别和操作的Mat数据格式
                Utils.bitmapToMat(bmp, mat);

                //Mat顺时针旋转90°
                Core.transpose(mat, mat);
                Core.flip(mat, mat, 1);

                Mat mat_out_gray = new Mat();


                //src为原图，dst为输出结果
                //将rgb图像转换成灰度图像
                Imgproc.cvtColor(mat, mat_out_gray, Imgproc.COLOR_RGB2GRAY);

                //捕捉棋盘格
                processFrame(mat_out_gray);

                // Update image
//                final Bitmap bitmap = Bitmap.createBitmap(mat_out_gray.cols(), mat_out_gray.rows(), Bitmap.Config.ARGB_8888);
//                Utils.matToBitmap(mat_out_gray, bitmap);
//                ivBitmap.setImageBitmap(bitmap);

                // Make sure we close the image

                if (mPatternWasFound) {

                    //Rvec：旋转矩阵，Tvec：平移矩阵
                    Tvec = new Mat(3, 1, CvType.CV_32FC1);
                    Rvec = new Mat(3, 3, CvType.CV_32FC1);

                    List<Point> imgPoints = new ArrayList<>();
                    List<Point3> objPoints = new ArrayList<>();

                    imgPoints.add(mCorners.toList().get(0));
                    imgPoints.add(mCorners.toList().get(1));
                    imgPoints.add(mCorners.toList().get(2));
                    imgPoints.add(mCorners.toList().get(6));
                    imgPoints.add(mCorners.toList().get(7));
                    imgPoints.add(mCorners.toList().get(8));
                    imgPoints.add(mCorners.toList().get(12));
                    imgPoints.add(mCorners.toList().get(13));
                    imgPoints.add(mCorners.toList().get(14));

                    Log.d(TAG, "imgPoints: " + imgPoints.toString());

                    //三维坐标单位为米，以opencv坐标系为参考
                    objPoints.add(new Point3(-0.02, -0.02, 0));//0
                    objPoints.add(new Point3(0, -0.02, 0));//1
                    objPoints.add(new Point3(0.02, -0.02, 0));//2
                    objPoints.add(new Point3(-0.02, 0, 0));//6
                    objPoints.add(new Point3(0., 0, 0));//7
                    objPoints.add(new Point3(0.02, 0, 0));//8
                    objPoints.add(new Point3(-0.02, 0.02, 0));//12
                    objPoints.add(new Point3(0, 0.02, 0));//13
                    objPoints.add(new Point3(0.02, 0.02, 0));//14

//                    objPoints.add(new Point3(-0.02, 0.02, 0));//0
//                    objPoints.add(new Point3(0, 0.02, 0));//1
//                    objPoints.add(new Point3(0.02, 0.02, 0));//2
//                    objPoints.add(new Point3(-0.02, 0, 0));//6
//                    objPoints.add(new Point3(0., 0, 0));//7
//                    objPoints.add(new Point3(0.02, 0, 0));//8
//                    objPoints.add(new Point3(-0.02, -0.02, 0));//12
//                    objPoints.add(new Point3(0, -0.02, 0));//13
//                    objPoints.add(new Point3(0.02, -0.02, 0));//14


                    MatOfPoint2f imgPointsMat = new MatOfPoint2f();
                    imgPointsMat.fromList(imgPoints);
                    MatOfPoint3f objPointsMat = new MatOfPoint3f();
                    objPointsMat.fromList(objPoints);

                    //内参矩阵
                    Mat cameraMatrix = new Mat(3, 3, CvType.CV_32FC1);
                    //畸变参数矩阵
                    MatOfDouble distorsionMatrix = new MatOfDouble();

                    double[] buff = {3402, 0, 911.5,
                            0, 3402, 1999.5,
                            0, 0, 1};

                    cameraMatrix.put(0, 0, buff);


                    double[] distArr = {
//                            0.21479880853937872,
//                            0.49445857491857304,
                            0,
                            0,
                            0,
                            0,
//                            -19.528481140479151
                            0
                    };

                    distorsionMatrix.fromArray(distArr);

                    Calib3d.solvePnP(objPointsMat, imgPointsMat, cameraMatrix, distorsionMatrix, Rvec, Tvec);

                    double[] rvec = new double[]{
                            -Rvec.get(0, 0)[0],
                            Rvec.get(1, 0)[0],
                            Rvec.get(2, 0)[0]
                    };

                    Rvec.put(0, 0, rvec);


                    Calib3d.Rodrigues(Rvec, rotMat);

                    double[] d = {
                            1, 0, 0,
                            0, -1, 0,
                            0, 0, -1
                    };

                    Mat rx = new Mat(3, 3, CvType.CV_32FC1);

                    rx.put(0, 0, d);

//                    Core.gemm(rx, rotMat, 1, new Mat(), 1, rotMat);
                    Core.gemm(rx, Tvec, 1, new Mat(), 1, Tvec);

                    for (int i = 0; i < 3; i++) {
                        for (int j = 0; j < 3; j++) {
                            Log.d(TAG, "rotMat: [" + i + "," + j + "]=" + Arrays.toString(rotMat.get(i, j)) + "");
                        }
                    }

                    Log.d(TAG, "Tvec:" + Arrays.toString(Tvec.get(0, 0)));
                    Log.d(TAG, "Tvec:" + Arrays.toString(Tvec.get(1, 0)));
                    Log.d(TAG, "Tvec:" + Arrays.toString(Tvec.get(2, 0)));

                    model_view_matrix[0] = (float) rotMat.get(0, 0)[0];
                    model_view_matrix[1] = (float) rotMat.get(0, 1)[0];
                    model_view_matrix[2] = (float) rotMat.get(0, 2)[0];
                    model_view_matrix[3] = 0;
                    model_view_matrix[4] = (float) rotMat.get(1, 0)[0];
                    model_view_matrix[5] = (float) rotMat.get(1, 1)[0];
                    model_view_matrix[6] = (float) rotMat.get(1, 2)[0];
                    model_view_matrix[7] = 0;
                    model_view_matrix[8] = (float) rotMat.get(2, 0)[0];
                    model_view_matrix[9] = (float) rotMat.get(2, 1)[0];
                    model_view_matrix[10] = (float) rotMat.get(2, 2)[0];
                    model_view_matrix[11] = 0;
                    model_view_matrix[12] = (float) Tvec.get(0, 0)[0];
                    model_view_matrix[13] = (float) Tvec.get(1, 0)[0];
                    model_view_matrix[14] = (float) Tvec.get(2, 0)[0];
                    model_view_matrix[15] = 1;

                    Log.d(TAG, "getFinalMatrix: model_view_matrix:" + Arrays.toString(model_view_matrix));

                }//if(mPatternWasFound) finish
            }
        });
    }

    /*******************************************************************/
    /**
     * opencv
     **/

    private void processFrame(Mat mat_out_gray) {

        //切换识别的是圆形棋盘格，还是方形棋盘格

        mPatternWasFound = Calib3d.findChessboardCorners(mat_out_gray, mPatternSize,
                mCorners, Calib3d.CALIB_CB_ADAPTIVE_THRESH);

//        renderFrame(mat_out_gray);//1920*864

        Log.d(TAG, "mCorners" + Arrays.toString(mCorners.toArray()));

    }


    private void renderFrame(Mat rgbaFrame) {
        //画出识别标识
//        drawPoints(rgbaFrame);
        List<Point> imgPoints = new ArrayList<Point>();

        imgPoints.add(new Point(0, 0));
        imgPoints.add(new Point(100, 100));
        imgPoints.add(mCorners.toList().get(7));
        imgPoints.add(mCorners.toList().get(13));
        imgPoints.add(mCorners.toList().get(18));

        Imgproc.drawMarker(rgbaFrame, imgPoints.get(0), new Scalar(255, 255, 0), 2, 50);
        Imgproc.drawMarker(rgbaFrame, imgPoints.get(1), new Scalar(255, 255, 0), 2, 50);
    }

    private void drawPoints(Mat rgbaFrame) {
        /*
         * image,棋盘格图像即是输入也是输出
         * patternSize,棋盘格内部角点的行、列数
         * corners,findChessboardCorners()输出的角点
         * patternWasFound,findChessboardCorners()的返回值
         * */
        Calib3d.drawChessboardCorners(rgbaFrame, mPatternSize, mCorners, mPatternWasFound);
    }

    /*******************************************************************/


    public void pingMuChange() {

//        Camera.Parameters parameters = mCamera.getParameters();
        // get Camera parameters

        parameters.setFocusMode("FOCUS_MODE_FIXED");

        // 设置图片格式
        parameters.setPictureFormat(ImageFormat.JPEG);
        // 设置JPG照片的质量
        parameters.set("jpeg-quality", 85);

        //1040 2850
//        parameters.setPreviewSize(864,1920);
//        parameters.setPreviewSize(1040, 2850);
        Log.d(TAG, "CAMERAINFO: " + parameters.getFocusMode() + "," + parameters.getFocalLength() + "," + parameters.getPreviewSize().width);
        parameters.setPreviewFrameRate(15);
        mCamera.startPreview();
    }


    public void NV21ToBitmap(Context context) {
        rs = RenderScript.create(context);
        yuvToRgbIntrinsic = ScriptIntrinsicYuvToRGB.create(rs, Element.U8_4(rs));
    }

    public Bitmap nv21ToBitmap(byte[] nv21, int width, int height) {
        if (yuvType == null) {
            yuvType = new Type.Builder(rs, Element.U8(rs)).setX(nv21.length);
            in = Allocation.createTyped(rs, yuvType.create(), Allocation.USAGE_SCRIPT);

            Type.Builder rgbaType = new Type.Builder(rs, Element.RGBA_8888(rs)).setX(width).setY(height);
            out = Allocation.createTyped(rs, rgbaType.create(), Allocation.USAGE_SCRIPT);
        }

        in.copyFrom(nv21);

        yuvToRgbIntrinsic.setInput(in);
        yuvToRgbIntrinsic.forEach(out);

        Bitmap bmpout = Bitmap.createBitmap(width, height, Bitmap.Config.ARGB_8888);
        out.copyTo(bmpout);
        return bmpout;
    }

    public float[] getModel_view_matrix() {
        return model_view_matrix;
    }

}
